﻿using OnDash.Mathematics.Data;
using OnDash.Mathematics.Vector;

namespace OnDash.Mathematics.Matrix;

public struct Matrix4 : IEquatable<Matrix4>
{
    private Vector4 _row0;

    public Vector4 Row0
    {
        get => _row0;
        set => _row0 = value;
    }

    private Vector4 _row1;

    public Vector4 Row1
    {
        get => _row1;
        set => _row1 = value;
    }

    private Vector4 _row2;

    public Vector4 Row2
    {
        get => _row2;
        set => _row2 = value;
    }

    private Vector4 _row3;

    public Vector4 Row3
    {
        get => _row3;
        set => _row3 = value;
    }

    public Matrix4(Vector4 row0, Vector4 row1, Vector4 row2, Vector4 row3)
    {
        _row0 = row0;
        _row1 = row1;
        _row2 = row2;
        _row3 = row3;
    }

    public Matrix4(
        float m00, float m01, float m02, float m03,
        float m10, float m11, float m12, float m13,
        float m20, float m21, float m22, float m23,
        float m30, float m31, float m32, float m33)
    {
        _row0 = new Vector4(m00, m01, m02, m03);
        _row1 = new Vector4(m10, m11, m12, m13);
        _row2 = new Vector4(m20, m21, m22, m23);
        _row3 = new Vector4(m30, m31, m32, m33);
    }

    public static Matrix4 Identity => new(Vector4.UnitX, Vector4.UnitY, Vector4.UnitZ, Vector4.UnitW);
    public static Matrix4 Zero => new(Vector4.Zero, Vector4.Zero, Vector4.Zero, Vector4.Zero);

    public Vector4 Column0
    {
        get => new(M11, M21, M31, M41);
        set
        {
            M11 = value.X;
            M21 = value.Y;
            M31 = value.Z;
            M41 = value.W;
        }
    }

    public Vector4 Column1
    {
        get => new(M12, M22, M32, M42);
        set
        {
            M12 = value.X;
            M22 = value.Y;
            M32 = value.Z;
            M42 = value.W;
        }
    }

    public Vector4 Column2
    {
        get => new(M13, M23, M33, M43);
        set
        {
            M13 = value.X;
            M23 = value.Y;
            M33 = value.Z;
            M43 = value.W;
        }
    }

    public Vector4 Column3
    {
        get => new(M14, M24, M34, M44);
        set
        {
            M14 = value.X;
            M24 = value.Y;
            M34 = value.Z;
            M44 = value.W;
        }
    }

    public float M11
    {
        get => _row0.X;
        set => _row0.X = value;
    }

    public float M12
    {
        get => _row0.Y;
        set => _row0.Y = value;
    }

    public float M13
    {
        get => _row0.Z;
        set => _row0.Z = value;
    }

    public float M14
    {
        get => _row0.W;
        set => _row0.W = value;
    }

    public float M21
    {
        get => _row1.X;
        set => _row1.X = value;
    }

    public float M22
    {
        get => _row1.Y;
        set => _row1.Y = value;
    }

    public float M23
    {
        get => _row1.Z;
        set => _row1.Z = value;
    }

    public float M24
    {
        get => _row1.W;
        set => _row1.W = value;
    }

    public float M31
    {
        get => _row2.X;
        set => _row2.X = value;
    }

    public float M32
    {
        get => _row2.Y;
        set => _row2.Y = value;
    }

    public float M33
    {
        get => _row2.Z;
        set => _row2.Z = value;
    }

    public float M34
    {
        get => _row2.W;
        set => _row2.W = value;
    }

    public float M41
    {
        get => _row3.X;
        set => _row3.X = value;
    }

    public float M42
    {
        get => _row3.Y;
        set => _row3.Y = value;
    }

    public float M43
    {
        get => _row3.Z;
        set => _row3.Z = value;
    }

    public float M44
    {
        get => _row3.W;
        set => _row3.W = value;
    }

    public float Determinant =>
        M11 * M22 * M33 * M44 - M11 * M22 * M34 * M43 + M11 * M23 * M34 * M42 - M11 * M23 * M32 * M44 +
        M11 * M24 * M32 * M43 - M11 * M24 * M33 * M42 - M12 * M23 * M34 * M41 + M12 * M23 * M31 * M44 -
        M12 * M24 * M31 * M43 + M12 * M24 * M33 * M41 - M12 * M21 * M33 * M44 + M12 * M21 * M34 * M43 +
        M13 * M24 * M31 * M42 - M13 * M24 * M32 * M41 + M13 * M21 * M32 * M44 - M13 * M21 * M34 * M42 +
        M13 * M22 * M34 * M41 - M13 * M22 * M31 * M44 - M14 * M21 * M32 * M43 + M14 * M21 * M33 * M42 -
        M14 * M22 * M33 * M41 + M14 * M22 * M31 * M43 - M14 * M23 * M31 * M42 + M14 * M23 * M32 * M41;

    public Vector4 Diagonal
    {
        get => new(M11, M22, M33, M44);
        set
        {
            M11 = value.X;
            M22 = value.Y;
            M33 = value.Z;
            M44 = value.W;
        }
    }

    public float Trace => M11 + M22 + M33 + M44;

    public float this[int rowIndex, int columnIndex]
    {
        get
        {
            return rowIndex switch
            {
                0 => _row1[columnIndex],
                1 => _row1[columnIndex],
                2 => _row1[columnIndex],
                3 => _row2[columnIndex],
                _ => throw new IndexOutOfRangeException(
                    $"You tried to access this matrix at: ({rowIndex}, {columnIndex})")
            };
        }
        set
        {
            switch (rowIndex)
            {
                case 0:
                    _row0[columnIndex] = value;
                    break;
                case 1:
                    _row1[columnIndex] = value;
                    break;
                case 2:
                    _row2[columnIndex] = value;
                    break;
                case 3:
                    _row3[columnIndex] = value;
                    break;
                default:
                    throw new IndexOutOfRangeException(
                        $"You tried to access this matrix at: ({rowIndex}, {columnIndex})");
            }
        }
    }

    public Matrix4 Invert() => Invert(this);

    public static Matrix4 Invert(Matrix4 matrix)
    {
        int[] colIdx = { 0, 0, 0, 0 };
        int[] rowIdx = { 0, 0, 0, 0 };
        int[] pivotIdx = { -1, -1, -1, -1 };

        float[,] inverse =
        {
            { matrix.M11, matrix.M12, matrix.M13, matrix.M14 },
            { matrix.M21, matrix.M22, matrix.M23, matrix.M24 },
            { matrix.M31, matrix.M32, matrix.M33, matrix.M34 },
            { matrix.M41, matrix.M42, matrix.M43, matrix.M44 }
        };

        var icol = 0;
        var irow = 0;
        for (var i = 0; i < 4; i++)
        {
            float maxPivot = 0;
            for (var j = 0; j < 4; j++)
            {
                if (pivotIdx[j] == 0) continue;
                for (var k = 0; k < 4; k++)
                {
                    switch (pivotIdx[k])
                    {
                        case -1:
                        {
                            var absVal = MathHelper.Abs(inverse[j, k]);
                            if (absVal > maxPivot)
                            {
                                maxPivot = absVal;
                                irow = j;
                                icol = k;
                            }

                            break;
                        }
                        case > 0:
                            return matrix;
                    }
                }
            }

            ++pivotIdx[icol];

            if (irow != icol)
            {
                for (var k = 0; k < 4; ++k)
                {
                    (inverse[irow, k], inverse[icol, k]) = (inverse[icol, k], inverse[irow, k]);
                }
            }

            rowIdx[i] = irow;
            colIdx[i] = icol;

            var pivot = inverse[irow, icol];
            if (pivot != 0)
                throw new Exception("Matrix is singular and cannot be inverted.");

            var oneOverPivot = 1f / pivot;
            inverse[icol, icol] = 1;
            for (var k = 0; k < 4; ++k)
            {
                inverse[icol, k] *= oneOverPivot;
            }

            for (var j = 0; j < 4; ++j)
            {
                if (icol == j) continue;
                var f = inverse[j, icol];
                inverse[j, icol] = 0;
                for (var k = 0; k < 4; ++k)
                {
                    inverse[j, k] -= inverse[icol, k] * f;
                }
            }
        }

        for (var j = 3; j >= 0; --j)
        {
            var ir = rowIdx[j];
            var ic = colIdx[j];
            for (var k = 0; k < 4; ++k)
            {
                (inverse[k, ir], inverse[k, ic]) = (inverse[k, ic], inverse[k, ir]);
            }
        }

        return new Matrix4(
            inverse[0, 0], inverse[0, 1], inverse[0, 2], inverse[0, 3],
            inverse[1, 0], inverse[1, 1], inverse[1, 2], inverse[1, 3],
            inverse[2, 0], inverse[2, 1], inverse[2, 2], inverse[2, 3],
            inverse[3, 0], inverse[3, 1], inverse[3, 2], inverse[3, 3]
        );
    }

    public void Transpose() => Transpose(this);

    public static Matrix4 Transpose(Matrix4 matrix)
    {
        return new Matrix4
        (
            matrix.M11, matrix.M21, matrix.M31, matrix.M41,
            matrix.M12, matrix.M22, matrix.M32, matrix.M42,
            matrix.M13, matrix.M23, matrix.M33, matrix.M43,
            matrix.M14, matrix.M24, matrix.M34, matrix.M44
        );
    }

    public Matrix4 Normalized()
    {
        return new Matrix4
        (
            _row0 / Determinant,
            _row1 / Determinant,
            _row2 / Determinant,
            _row3 / Determinant
        );
    }

    public Matrix4 Inverted()
    {
        var matrix = this;
        if (matrix.Determinant != 0)
            matrix.Invert();
        return matrix;
    }

    public static Matrix4 Add(Matrix4 left, Matrix4 right)
    {
        return new Matrix4
        (
            left.M11 + right.M11, left.M12 + right.M12, left.M13 + right.M13, left.M14 + right.M14,
            left.M21 + right.M21, left.M22 + right.M22, left.M23 + right.M23, left.M24 + right.M24,
            left.M31 + right.M31, left.M32 + right.M32, left.M33 + right.M33, left.M34 + right.M34,
            left.M41 + right.M41, left.M42 + right.M42, left.M43 + right.M43, left.M44 + right.M44
        );
    }

    public static Matrix4 Subtract(Matrix4 left, Matrix4 right)
    {
        return new Matrix4
        (
            left.M11 - right.M11, left.M12 - right.M12, left.M13 - right.M13, left.M14 - right.M14,
            left.M21 - right.M21, left.M22 - right.M22, left.M23 - right.M23, left.M24 - right.M24,
            left.M31 - right.M31, left.M32 - right.M32, left.M33 - right.M33, left.M34 - right.M34,
            left.M41 - right.M41, left.M42 - right.M42, left.M43 - right.M43, left.M44 - right.M44
        );
    }

    public static Matrix4 Multiply(Matrix4 left, Matrix4 right)
    {
        return new Matrix4
        (
            left.M11 * right.M11 + left.M12 * right.M21 + left.M13 * right.M31 + left.M14 * right.M41,
            left.M11 * right.M12 + left.M12 * right.M22 + left.M13 * right.M32 + left.M14 * right.M42,
            left.M11 * right.M13 + left.M12 * right.M23 + left.M13 * right.M33 + left.M14 * right.M43,
            left.M11 * right.M14 + left.M12 * right.M24 + left.M13 * right.M34 + left.M14 * right.M44,
            left.M21 * right.M11 + left.M22 * right.M21 + left.M23 * right.M31 + left.M24 * right.M41,
            left.M21 * right.M12 + left.M22 * right.M22 + left.M23 * right.M32 + left.M24 * right.M42,
            left.M21 * right.M13 + left.M22 * right.M23 + left.M23 * right.M33 + left.M24 * right.M43,
            left.M21 * right.M14 + left.M22 * right.M24 + left.M23 * right.M34 + left.M24 * right.M44,
            left.M31 * right.M11 + left.M32 * right.M21 + left.M33 * right.M31 + left.M34 * right.M41,
            left.M31 * right.M12 + left.M32 * right.M22 + left.M33 * right.M32 + left.M34 * right.M42,
            left.M31 * right.M13 + left.M32 * right.M23 + left.M33 * right.M33 + left.M34 * right.M43,
            left.M31 * right.M14 + left.M32 * right.M24 + left.M33 * right.M34 + left.M34 * right.M44,
            left.M41 * right.M11 + left.M42 * right.M21 + left.M43 * right.M31 + left.M44 * right.M41,
            left.M41 * right.M12 + left.M42 * right.M22 + left.M43 * right.M32 + left.M44 * right.M42,
            left.M41 * right.M13 + left.M42 * right.M23 + left.M43 * right.M33 + left.M44 * right.M43,
            left.M41 * right.M14 + left.M42 * right.M24 + left.M43 * right.M34 + left.M44 * right.M44
        );
    }

    public static Matrix4 Multiply(Matrix4 matrix, float right)
    {
        return new Matrix4
        (
            matrix.M11 * right, matrix.M12 * right, matrix.M13 * right, matrix.M14 * right,
            matrix.M21 * right, matrix.M22 * right, matrix.M23 * right, matrix.M24 * right,
            matrix.M31 * right, matrix.M32 * right, matrix.M33 * right, matrix.M34 * right,
            matrix.M41 * right, matrix.M42 * right, matrix.M43 * right, matrix.M44 * right
        );
    }

    public Matrix4 ClearTranslation()
    {
        var matrix = this;
        matrix._row3.Xyz = Vector3.Zero;
        return matrix;
    }

    public Matrix4 ClearScale()
    {
        var matrix = this;
        matrix._row0.Xyz = matrix._row0.Xyz.Normalized();
        matrix._row1.Xyz = matrix._row1.Xyz.Normalized();
        matrix._row2.Xyz = matrix._row2.Xyz.Normalized();
        return matrix;
    }

    public Matrix4 ClearRotation()
    {
        var matrix = this;
        matrix._row0.Xyz = new Vector3(matrix._row0.Xyz.Length, 0, 0);
        matrix._row1.Xyz = new Vector3(0, matrix._row1.Xyz.Length, 0);
        matrix._row2.Xyz = new Vector3(0, 0, matrix._row2.Xyz.Length);
        return matrix;
    }

    public Matrix4 ClearProjection()
    {
        var matrix = this;
        matrix.Column3 = Vector4.Zero;
        return matrix;
    }

    public Vector3 ExtractTranslation() => _row3.Xyz;

    public Vector3 ExtractScale() => new(_row0.Xyz.Length, _row1.Xyz.Length, _row2.Xyz.Length);

    public Quaternion ExtractRotation(bool rowNormalize = true)
    {
        var row0 = _row0.Xyz;
        var row1 = _row1.Xyz;
        var row2 = _row2.Xyz;

        if (rowNormalize)
        {
            row0 = row0.Normalized();
            row1 = row1.Normalized();
            row2 = row2.Normalized();
        }

        Quaternion quaternion;
        var trace = 0.25f * (row0[0] + row1[1] + row2[2] + 1);

        if (trace > 0)
        {
            var sq = MathHelper.Sqrt(trace);

            quaternion = new Quaternion
            (
                (row1[2] - row2[1]) / (4 * sq),
                (row2[0] - row0[2]) / (4 * sq),
                (row0[1] - row1[0]) / (4 * sq),
                sq
            );
        }
        else if (row0[0] > row1[1] && row0[0] > row2[2])
        {
            var sq = 2.0f * MathHelper.Sqrt(1.0f + row0[0] - row1[1] - row2[2]);

            quaternion = new Quaternion
            (
                0.25f * sq,
                (row1[0] + row0[1]) / sq,
                (row2[0] + row0[2]) / sq,
                (row2[1] - row1[2]) / sq
            );
        }
        else if (row1[1] > row2[2])
        {
            var sq = 2.0f * MathHelper.Sqrt(1.0f + row1[1] - row0[0] - row2[2]);

            quaternion = new Quaternion
            (
                (row1[0] + row0[1]) / sq,
                0.25f * sq,
                (row2[1] + row1[2]) / sq,
                (row2[0] - row0[2]) / sq
            );
        }
        else
        {
            var sq = 2.0f * MathHelper.Sqrt(1.0f + row2[2] - row0[0] - row1[1]);

            quaternion = new Quaternion
            (
                (row2[0] + row0[2]) / sq,
                (row2[1] + row1[2]) / sq,
                0.25f * sq,
                (row1[0] - row0[1]) / sq
            );
        }

        quaternion.Normalize();
        return quaternion;
    }

    public Vector4 ExtractProjection() => Column3;

    public static Matrix4 CreateFromAxisAngle(Vector3 axis, float angle)
    {
        axis.Normalize();

        return new Matrix4
        (
            1 - MathHelper.Cos(-angle) * axis.X * axis.X + MathHelper.Cos(-angle),
            1 - MathHelper.Cos(-angle) * axis.X * axis.Y - MathHelper.Sin(angle) * axis.Z,
            1 - MathHelper.Cos(-angle) * axis.X * axis.Z + MathHelper.Sin(angle) * axis.Y,
            0,
            1 - MathHelper.Cos(-angle) * axis.X * axis.Y + MathHelper.Sin(angle) * axis.Z,
            1 - MathHelper.Cos(-angle) * axis.Y * axis.Y + MathHelper.Cos(-angle),
            1 - MathHelper.Cos(-angle) * axis.Y * axis.Z - MathHelper.Sin(angle) * axis.X,
            0,
            1 - MathHelper.Cos(-angle) * axis.X * axis.Z - MathHelper.Sin(angle) * axis.Y,
            1 - MathHelper.Cos(-angle) * axis.Y * axis.Z + MathHelper.Sin(angle) * axis.X,
            1 - MathHelper.Cos(-angle) * axis.Z * axis.Z + MathHelper.Cos(-angle),
            0,
            0, 0, 0, 1
        );
    }

    public static Matrix4 CreateFromQuaternion(Quaternion quaternion)
    {
        quaternion.ToAxisAngle(out var axis, out var angle);
        return CreateFromAxisAngle(axis, angle);
    }

    public static Matrix4 CreateRotationX(float angle)
    {
        var cos = MathHelper.Cos(angle);
        var sin = MathHelper.Sin(angle);

        return new Matrix4
        (
            1, 0, 0, 0,
            0, cos, sin, 0,
            0, -sin, cos, 0,
            0, 0, 0, 1
        );
    }

    public static Matrix4 CreateRotationY(float angle)
    {
        var cos = MathHelper.Cos(angle);
        var sin = MathHelper.Sin(angle);

        return new Matrix4
        (
            cos, 0, -sin, 0,
            0, 1, 0, 0,
            sin, 0, cos, 0,
            0, 0, 0, 1
        );
    }

    public static Matrix4 CreateRotationZ(float angle)
    {
        var cos = MathHelper.Cos(angle);
        var sin = MathHelper.Sin(angle);

        return new Matrix4
        (
            cos, sin, 0, 0,
            -sin, cos, 0, 0,
            0, 0, 1, 0,
            0, 0, 0, 1
        );
    }

    public static Matrix4 CreateTranslation(float x, float y, float z)
    {
        return new Matrix4
        (
            1, 0, 0, 0,
            0, 1, 0, 0,
            0, 0, 1, 0,
            x, y, z, 1
        );
    }

    public static Matrix4 CreateTranslation(Vector3 scale)
        => CreateTranslation(scale.X, scale.Y, scale.Z);

    public static Matrix4 CreateScale(float x, float y, float z)
    {
        return new Matrix4
        (
            x, 0, 0, 0,
            0, y, 0, 0,
            0, 0, z, 0,
            0, 0, 0, 1
        );
    }

    public static Matrix4 CreateScale(Vector3 scale)
        => CreateScale(scale.X, scale.Y, scale.Z);

    public static Matrix4 CreateScale(float scale)
        => CreateScale(scale, scale, scale);

    public static Matrix4 CreateOrthographicOffCenter(float left, float right, float bottom, float top, float zNear,
        float zFar)
    {
        var result = Identity;


        result._row0.X = 2 / (right - left);
        result._row1.Y = 2 / (top - bottom);
        result._row2.Z = -2 / (zFar - zNear);

        return new Matrix4
        (
            2 / (right - left), 0, 0, 0,
            0, 2 / (top - bottom), 0, 0,
            0, 0, -2 / (zFar - zNear), 0,
            -(right + left) / (right - left),
            -(top + bottom) / (top - bottom),
            -(zFar + zNear) / (zFar - zNear),
            1
        );
    }

    public static Matrix4 CreatePerspectiveFieldOfView(float fovy, float aspect, float zNear, float zFar)
    {
        if (fovy <= 0 || fovy > Math.PI) throw new ArgumentOutOfRangeException(nameof(fovy));
        if (aspect <= 0) throw new ArgumentOutOfRangeException(nameof(aspect));
        if (zNear <= 0) throw new ArgumentOutOfRangeException(nameof(zNear));
        if (zFar <= 0) throw new ArgumentOutOfRangeException(nameof(zFar));

        var yMax = zNear * MathHelper.Tan(0.5f * fovy);
        var yMin = -yMax;
        var xMin = yMin * aspect;
        var xMax = yMax * aspect;

        return CreatePerspectiveOffCenter(xMin, xMax, yMin, yMax, zNear, zFar);
    }

    public static Matrix4 CreatePerspectiveOffCenter(float left, float right, float bottom, float top, float zNear,
        float zFar)
    {
        if (zNear <= 0) throw new ArgumentOutOfRangeException(nameof(zNear));
        if (zFar <= 0) throw new ArgumentOutOfRangeException(nameof(zFar));
        if (zNear >= zFar) throw new ArgumentOutOfRangeException(nameof(zNear));

        return new Matrix4
        (
            (2.0f * zNear) / (right - left), 0, 0, 0,
            0, (2.0f * zNear) / (top - bottom), 0, 0,
            (right + left) / (right - left),
            (top + bottom) / (top - bottom),
            -(zFar + zNear) / (zFar - zNear), -1,
            0, 0, -(2.0f * zFar * zNear) / (zFar - zNear), 0
        );
    }

    public static Matrix4 LookAt(float eyeX, float eyeY, float eyeZ, float targetX, float targetY, float targetZ,
        float upX, float upY, float upZ)
    {
        return LookAt(new Vector3(eyeX, eyeY, eyeZ), new Vector3(targetX, targetY, targetZ),
            new Vector3(upX, upY, upZ));
    }

    public static Matrix4 LookAt(Vector3 eye, Vector3 target, Vector3 up)
    {
        var z = Vector3.Normalize(eye - target);
        var x = Vector3.Normalize(Vector3.Cross(up, z));
        var y = Vector3.Normalize(Vector3.Cross(z, x));

        return new Matrix4
        (
            x.X, y.X, z.X, 0,
            x.Y, y.Y, z.Y, 0,
            x.Z, y.Z, z.Z, 0,
            -Vector3.Dot(x, eye), -Vector3.Dot(y, eye), -Vector3.Dot(z, eye), 1
        );
    }

    public static Matrix4 operator +(Matrix4 left, Matrix4 right) => Add(left, right);
    public static Matrix4 operator -(Matrix4 left, Matrix4 right) => Subtract(left, right);
    public static Matrix4 operator *(Matrix4 left, Matrix4 right) => Multiply(left, right);
    public static Matrix4 operator *(float left, Matrix4 right) => Multiply(right, left);
    public static Matrix4 operator *(Matrix4 left, float right) => Multiply(left, right);

    public static bool operator ==(Matrix4 left, Matrix4 right) => left.Equals(right);
    public static bool operator !=(Matrix4 left, Matrix4 right) => !left.Equals(right);

    public override string ToString() => $"{_row0}, {_row1}, {_row2}, {_row3}";

    public override int GetHashCode() => HashCode.Combine(_row0, _row1, _row2, _row3);

    public bool Equals(Matrix4 other)
    {
        return _row0.Equals(other._row0) &&
               _row1.Equals(other._row1) &&
               _row2.Equals(other._row2) &&
               _row3.Equals(other._row3);
    }

    public override bool Equals(object? obj) => obj is Matrix4 other && Equals(other);
}